# -*- coding:utf-8 -*-
from ctypes import *
import platform
import time
import numpy as np
import math
import pysoem
import os
from multiprocessing import Process, Manager


# 导入自定义函数和CANFD SDK动态链接库>>>
from os.path import dirname, join
import sys
sys.path.append(join(dirname(__file__), "src"))
sys.path.append(dirname(__file__))
from src import *

# ###########################################################################
time1 = time.time()

# const
control_time = 20
sensor_scaling = 0.01



def motor_process(shared_data):
    # 设置can or canfd接口
    lcanlib = LCAN()
    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, 0, 0)

    # open device
    ret = lcanlib.LCAN_OpenDevice(LCAN_USBCANFDMini, 0)
    if ret != LCAN_STATUS_OK:
        print("LYS USBCANFDMini Open Device failed!")
        exit(0)
    # open success
    print("LYS USBCANFDMini Open Device Success!")

    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, 0, 0)

    # 初始化 canfd 通信参数
    canfd_init_cfg = LCAN_INITFD_CONFIG()
    canfd_init_cfg.abitBaudHz = 1000000
    canfd_init_cfg.abit_timing = 0x00018B2E
    canfd_init_cfg.dbitBaudHz = 5000000
    canfd_init_cfg.dbit_timing = 0x00010207
    # normal
    canfd_init_cfg.mode = 0
    # CANFD enabled
    canfd_init_cfg.fdEn = 1
    # ISO CANFD enabled
    canfd_init_cfg.isoEn = 1

    canfd_init_cfg.rev1 = 0

    # 初始化canfd
    ret = lcanlib.LCAN_InitCANFD(LCAN_USBCANFDMini, 0, 0, canfd_init_cfg)
    if ret != LCAN_STATUS_OK:
        print("LYS USBCANmini Init Failed!")
    else:
        print("LYS USBCANmini Init Success!")
    print('sleeping')
    time.sleep(0.1)

    # 清除can通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, 0, 0)

    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    

    # 清除wh motor错误
    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, 0, 0, can_clear_err, 1)
    if ret != LCAN_STATUS_OK:
        print("clear err Failed!")
    else:
        print("clear err Success!")

    # 上使能
    start = time.time()
    while time.time() - start < 0.1:
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, 0, 0, can_enable, 1)
    if ret != LCAN_STATUS_OK:
        print("Enable Failed!")
    else:
        print("Enable Success!")

    # 在线更新标志位
    start = time.time()
    while time.time() - start < 0.1:
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, 0, 0, can_obj_init, 1)
    
    if ret != LCAN_STATUS_OK:
        print("motor init Failed!")
    else:
        print("motor init Success!")





    # 设置电流模式
    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, 0, 0, can_c_mode, 1)
    if ret != LCAN_STATUS_OK:
        print("Set c mode failed!")
    else:
        print("Set c mode success!")

    print('sleeping')
    time.sleep(0.1)

    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # 控制
    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, 0, 0)

    c = 0
    v0 = 0
    v1 = 0
    t_ext = 0
    start = time.time()
    time0 = start
    while time.time() - start < control_time:
        
        time1 = time.time()

        hex_c = decimal_to_four_hex_numbers(c)
        # print(f"hex_c is {hex_c}")
        can_set_c = LCAN_CANFD_OBJ()
        can_set_c.id = 0x401
        can_set_c.dataLen = 4
        can_set_c.data = (c_ubyte * 64)(hex_c[0], hex_c[1], hex_c[2], hex_c[3])
        # move
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, 0, 0, can_set_c, 1)
        # print("ret_c is ", ret)
        pvc = receive_pvc(lcanlib)
        
        p = pvc[0]
        if p is None:
            continue
        
        v0 = v1
        v = pvc[1]
        v1 = v
        c = pvc[2]
        
        a = (v1 - v0)/(time1 - time0) # 加速度从v差分得来
        a_d = 0

        p_d_rad = -math.pi/2
        p_d = round(p_d_rad/math.pi*180*10000)
        p_d = -000000
        v_d = 0
        
        # 获取 force_list
        force_list = shared_data['force_list']
        if force_list is not None:
            t_ext = force_list[5]*sensor_scaling
        # print(f"force_list from test.py: {force_list}")
        os.system('clear')
        print(f"t_ext is {t_ext}")
        print(f"v is {v}")

        
        # 系数
        kp = 0.005/10000 # Stiffness factor
        kv = 22/10000 # Damping factor 补偿
        ka = 0.1/10000 # Mass factor
        kvt = 0 # 摩擦补偿
        k_e = 30
        
        t = k_e*kp*(p_d - p) + kv*(v) + kvt*sign_func(v, 50) + ka*(a) + k_e*t_ext
        c = trans_t2c(t)
        # print("t_here is ", t)
        # print("c_here is ", c)
        time0 = time1
        
    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    c = 0
    start = time.time()
    while time.time() - start < 0.01:
        hex_c = decimal_to_four_hex_numbers(c)
        can_set_c.data = (c_ubyte * 64)(hex_c[0], hex_c[1], hex_c[2], hex_c[3])
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, 0, 0, can_set_c, 1)
        # print("c is 0")



    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, 0, 0)
    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    #Close Device
    lcanlib.LCAN_CloseDevice(LCAN_USBCANFDMini, 0)
    print("LYS USBCANmini Close!")

def sensor_process(shared_data):
    master = pysoem.Master()
    master.open('eth1')
    master.config_dc()

    if master.config_init() > 0:
        master.config_map()
        
        for slave in master.slaves:
            print(f"slave name is {slave.name}")
            print(f"slave state is {slave.state}")
            print(f"slave id is {slave.id}")

        master.state_check(pysoem.SAFEOP_STATE, 50000)
        master.state_check(pysoem.OP_STATE, 50000)
        master.write_state()
        master.in_op = True

        # 循环读取sensor>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
        start_time = time.time()
        while True:
            master.send_processdata()
            master.receive_processdata(2000)
            slave = master.slaves[0]
            
            # 假设 NET_1 到 NET_16 每个数据占用 4 字节 (32 位)
            data_offset = 0
            data_length = 4
            
            force_list = np.zeros(16, dtype=int)
            for i in range(1, 17):
                data_end = data_offset + data_length
                value = int.from_bytes(slave.input[data_offset:data_end], byteorder='little', signed=True)
                force_list[i-1] = value
                # print(f"NET_{i}: {value}")
                data_offset += data_length
            if time.time() - start_time > (control_time+2):
                break

            shared_data['force_list'] = force_list
            # print(f"force_list from test.py: {force_list}")
            print(f"t_ext = {force_list[5]}")
            # 添加一个小延迟以避免 CPU 占用过高
            time.sleep(0.0006)
        # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<  
        print("sensor end")

        master.state = pysoem.INIT_STATE
        master.write_state()
        master.close()

if __name__ == "__main__":
    with Manager() as manager:
        shared_data = manager.dict()
        shared_data['force_list'] = None
        process_motor = Process(target=motor_process, args=(shared_data,))
        process_sensor = Process(target=sensor_process, args=(shared_data,))
        process_motor.start()
        process_sensor.start()
        process_motor.join()
        process_sensor.join()